//
// Created by dwx on 2024/4/19.
//
#include "main.h"
#include "can.h"
#include "dma.h"
#include "usart.h"
#include "usb_device.h"
#include "gpio.h"
#include "FreeRTOS.h"
#include "task.h"
#include "mavlink_crc.h"

#define  DEM_CR      *(volatile uint32_t *)0xE000EDFC
#define  DWT_CR      *(volatile uint32_t *)0xE0001000
#define  DWT_CYCCNT  *(volatile uint32_t *)0xE0001004
#define  DEM_CR_TRCENA                   (1 << 24)
#define  DWT_CR_CYCCNTENA                (1 <<  0)




void DWT_Init(void)
{
    DEM_CR  |=  DEM_CR_TRCENA; /*对DEMCR寄存器的�?24控制，写1使能DWT外设�?*/
    DWT_CYCCNT = 0;/*对于DWT的CYCCNT计数寄存器清0�?*/
    DWT_CR  |=  DWT_CR_CYCCNTENA;/*对DWT控制寄存器的�?0控制，写1使能CYCCNT寄存器�??*/
}

void DWT_DelayUS(uint32_t _ulDelayTime)
{
    uint32_t tCnt, tDelayCnt;
    uint32_t tStart;
    vPortEnterCritical();
    tStart = DWT_CYCCNT; /* 刚进入时的计数器�? */
    tCnt = 0;
    tDelayCnt = _ulDelayTime * (SystemCoreClock / 1000000);
    /* �?要的节拍�? */    /*SystemCoreClock :系统时钟频率*/

    while(tCnt < tDelayCnt)
    {
        tCnt = DWT_CYCCNT - tStart;
        /* 求减过程中，如果发生第一�?32位计数器重新计数，依然可以正确计�? */
    }
    vPortExitCritical();
}

void DWT_DelayMS(uint32_t _ulDelayTime)
{
    DWT_DelayUS(1000*_ulDelayTime);
}



extern DataRec Uart1RecData;
extern DataRec USBRecData;

extern CAN_TxHeaderTypeDef TxMessage; //CAN发送的消息的消息头
extern CAN_RxHeaderTypeDef RxMessage; //CAN接收的消息的消息头

extern uint8_t  CanRecFlg;
extern void CAN1Filter_Config(void);
extern void CAN_Start_Init(void);
IRC_SIGNAL irc_data[5];



uint8_t *motrtype_name[] = {   "HARD_VER_ERR" ,
        "DUMMY",
        "AR3",
        "SITO",
        "BATTLE_CHARGE_THERE_AXIS",
        "MR6",
        "TRANSMIT_PLANE_THERE_AXIS",
        "HARD_VER_SUM_IN_THIS_SOFT_VER",
        "IMU6",
        "IRC",};



void SetMotorAngle(uint8_t _id,const uint8_t mode,float _angle,float motorspeede){
    CAN_TxHeaderTypeDef txHeader;

    uint32_t              TxMailbox;
    static uint8_t CanBuf[8] ;
    txHeader.StdId = _id << 7 | mode;

    txHeader.ExtId = 0;
    txHeader.IDE = CAN_ID_STD;
    txHeader.RTR = CAN_RTR_DATA;
    txHeader.DLC = 8;
    txHeader.TransmitGlobalTime = DISABLE;
    if(mode == 1 || mode == 0x50 ){
        CanBuf[0] = (int32_t)_angle;
    }else {
        auto *angle = (unsigned char *)&_angle;
        auto *speed = (unsigned char *)&motorspeede;
        for(int i = 0;i<4;i++){
            CanBuf[i] = *(angle+i);
            CanBuf[i+4] = *(speed+i);
        }
    }




    HAL_CAN_AddTxMessage(&hcan1, &txHeader, CanBuf, &TxMailbox) ;


}

void EnableMotor(void){
    //for(int i = 1;i<6;i++){
        SetMotorAngle(5,0x01,0x01,0x0);
        SetMotorAngle(5,0x03,3.3f,0x0);
        SetMotorAngle(5,0x04,0x00,0x0);
        SetMotorAngle(5,0x23,0x00,0x0);

        DWT_DelayMS(2);
        SetMotorAngle(5,0x15,0x01,0x0);
        DWT_DelayMS(2);
   // }
}
uint8_t  irc_dir = 0;
uint8_t CheckCan = 0;
extern CanRecData CanData;

uint8_t  GetBitValue(uint8_t data,uint8_t index){
    return (data&(0x1<<index))>>index;
}
void CheckPlaneData(uint8_t Msg_Type,uint16_t param1,uint8_t check_cmd,uint8_t ask_result,uint16_t datalength,uint8_t takeon){
    static  uint8_t buff[1024];
    uint16_t length = datalength+8;
    static uint16_t crc = 0;
    buff[0] = 0xfe;
    buff[1] = datalength;
    buff[3] = 0x01;
    buff[4] = 0x02;
    buff[5] = Msg_Type;
    buff[14] = param1;
    buff[15] = param1>>8&0xff;
    buff[30] = check_cmd;
    buff[31] = ask_result;
    buff[33] = takeon;
    crc = generate_vector(buff,length-2, buff[5] );
    buff[datalength+6] = crc&0xff;
    buff[datalength+7] = (crc>>8)&0xff;
    HAL_UART_Transmit(&huart1,buff,length,HAL_MAX_DELAY);
}
float ZeroData[5] = {0.0f,0.0f,0.0f,0.0f,0.0f};
uint8_t  ReadPosFLG = 0;
void usbrec(void *pvParameters)
{
    uint16_t cnt = 0;
    while(1)
    {
        if(USBRecData.recflag){
            if(USBRecData.buff[0] == '#' && USBRecData.buff[USBRecData.reclen-1] == '#'){
                if(strstr((char*)USBRecData.buff,"Open IRC front")){
                    irc_dir = 1;
                    CheckPlaneData(208,0x0001,56,0x01,0x1c,irc_dir);
                }
                if(strstr((char*)USBRecData.buff,"Open IRC behind")){
                    irc_dir = 2;
                    CheckPlaneData(208,0x0002,56,0x00,0x1c,irc_dir);
                }
                if(strstr((char*)USBRecData.buff,"Open IRC left")){
                    irc_dir = 3;
                    CheckPlaneData(208,0x0004,56,0x00,0x1c, irc_dir );
                }
                if(strstr((char*)USBRecData.buff,"Open IRC right")){
                    irc_dir = 4;
                    CheckPlaneData(208,0x0008,56,0x00,0x1c,irc_dir);
                }
                if(strstr((char*)USBRecData.buff,"Close IRC")){
                    //CheckPlaneData(208,0x0000,56,0x01,0x1c);
                    irc_dir = 0;
                    CheckPlaneData(208,0x0000,41,0x00,0x1c,irc_dir );
                }
                if(strstr((char*)USBRecData.buff,"Check System Version")){
                    CheckCan = 1;
                    printf("Main Board Version:%d.%d.%d\n",SystemVersion_1,SystemVersion_2,SystemVersion_3);
                }

            }
            if(USBRecData.buff[0] == '!'){
                if(strstr(&USBRecData.buff[1],"M[J]") != NULL) {
                    uint8_t argNum,id;
                    argNum = sscanf(USBRecData.buff, "! M[J] %d", &id);
                    printf("argNum = %d,id=%d\n",argNum,id);
                    if(argNum == 1){
                        SetMotorAngle(id,0x02,0.0f,1);
                    }
                    // Float to Bytes

                }
                if(strstr(&USBRecData.buff[1],"Set Pos:") != NULL) {
                    uint8_t argNum;
                    float Value[3];
                    argNum = sscanf(USBRecData.buff, "! Set Pos:%f,%f,%f",&Value[0],&Value[1],&Value[2]);
                    printf("argNum = %d,1:%f,2:%f,3:%f\n",argNum,Value[0],Value[1],Value[2]);
                    if(argNum == 3){
                        for(int i=0;i<argNum;i++){
                            SetMotorAngle(i+1,0x08,Value[i],1);
                        }
                    }
                }
                if(strstr(&USBRecData.buff[1],"M[A]") != NULL) {
                    uint8_t argNum,id=0;
                    float Value;
                    argNum = sscanf(USBRecData.buff, "! M[A] %d,%f", &id,&Value);
                    printf("argNum = %d,id=%d,CharngData = %f,ZeroData[argNum-1] = %f\n",argNum,id,Value,ZeroData[id-1]);
                    if(argNum == 2){

                        ZeroData[id-1] =  ZeroData[id-1]+(Value*1.0f);
                        SetMotorAngle(id,0x08,ZeroData[id-1],10);

                    }
                }
                if(strstr(&USBRecData.buff[1],"Read Pos?") != NULL) {
                    ReadPosFLG = 1;
                    for(int i = 0;i<5;i++){
                        SetMotorAngle(i+1,0x09,0,1);
                    }

                }
                if(strstr(&USBRecData.buff[1],"Read Signal:") != NULL) {
                    uint8_t argNum,id=0;
                    argNum = sscanf(USBRecData.buff, "! Read Signal:%d", &id);
                    printf("argNum = %d,id=%d\n",argNum,id);
                    if(argNum == 1){
                        SetMotorAngle(id,0x50,0,1);
                    }

                }
                if(strstr(&USBRecData.buff[1],"Start Work") != NULL) {

                    SetMotorAngle(1,0x51,0,1);

                }
                if(strstr(&USBRecData.buff[1],"Stop Work") != NULL){

                }
                if(strstr(&USBRecData.buff[1],"Read Color?") != NULL) {
                    SetMotorAngle(3,0x51,0,0);

                }
            }

            memset(USBRecData.buff,0,sizeof (USBRecData.buff));
            USBRecData.reclen = 0;
            USBRecData.recflag = 0;
        }
        vTaskDelay(1);
    }
}
void keyscan(void *pvParameters)
{
    uint16_t cnt = 0;
    while(1)
    {

        if(cnt++>=100){
            cnt = 0;
            //printf("Key1:%d,Key2:%d\n",ReadKey1,ReadKey2);
        }
        vTaskDelay(1);
    }
}
void Uart1Recentry(void *arg){
    uint16_t cnt = 0;
    uint16_t clscdata = 0xffff,recdata = 0xffff;
    while(1){
        if(Uart1RecData.recflag){


            if(Uart1RecData.reclen == 4 && Uart1RecData.buff[0] == 0xee && Uart1RecData.buff[1] == 0xee&&Uart1RecData.buff[2] == 0xee&&Uart1RecData.buff[3] ==0xee){
                //printf("not Plane Data\n");
            }else {
                if(Uart1RecData.buff[5] == 206) {
                    clscdata = generate_vector(Uart1RecData.buff, Uart1RecData.reclen - 2, Uart1RecData.buff[5]);
                    recdata =
                            Uart1RecData.buff[Uart1RecData.reclen - 1] << 8 | Uart1RecData.buff[Uart1RecData.reclen - 2];
                    if(clscdata == recdata){
                        if(irc_dir > 0 ){
                            //printf("staut:0x%02X\n",GetBitValue(Uart1RecData.buff[Uart1RecData.reclen - 4],irc_dir-1));
                        }//
                    }

                }
            }
            Uart1RecData.reclen = 0;
            memset(Uart1RecData.buff,0,sizeof (Uart1RecData.buff));
            Uart1RecData.recflag = 0;
        }
        vTaskDelay(1);
    }
}
void CanEntry(void *arg){
    uint16_t cnt = 0;
    uint8_t id = 1;
    static uint8_t Open_Motor = 0;

    while(1){
        if(Open_Motor == 0){
            EnableMotor();
            Open_Motor = 1;

        }
        if(Open_Motor){
            if(CheckCan ){
                SetMotorAngle(id,0x32,0,0);
                if(id++>=6){
                    id = 0;
                    CheckCan = 0;
                }
            }
            if(CanRecFlg) {
                CanRecFlg = 0;
                switch (CanData.cmd) {
                    case 0x32:
                        printf("Montor:%d  Type:%s Version:%d.%d\n", CanData.id, motrtype_name[CanData.CanRecdata[0]], CanData.CanRecdata[3], CanData.CanRecdata[4]);
                        break;
                    case 0x23:
                        irc_data[CanData.id-1].data = *(float *) (CanData.CanRecdata);
                        if(ReadPosFLG){
                            ReadPosFLG = 0;
                            printf("M1:%f,M2:%f,M3:%f,M4:%f,M5:%f\n", irc_data[0].data, irc_data[1].data, irc_data[2].data,irc_data[3].data,irc_data[4].data);
                        }

                        break;
                }

            }
        }

        vTaskDelay(1);
    }
}


void Main(void){
    DWT_Init();
    xTaskCreate(usbrec, "usbrec", 512, NULL, 1, NULL);
    xTaskCreate(keyscan, "keyscan", 256, NULL, 2, NULL);
    xTaskCreate(Uart1Recentry, "Uart1Rec", 1024, NULL, 3, NULL);
    xTaskCreate(CanEntry, "Can", 512, NULL, 1, NULL);
    vTaskStartScheduler();          //¿ªÆôÈÎÎñµ÷¶È

}
